#include "RadarController.h"
#include <QTimer>
#include <string>

RadarController::RadarController(QHostAddress ip, int port, QObject *parent)
    :QObject(parent)
    , m_ip(ip)
    , m_port(port)
{
    m_tcpClient = new TcpClient(this);
    connect(m_tcpClient, &TcpClient::connected, this, &RadarController::login);
    connect(m_tcpClient, &TcpClient::recvHeartbeatResponse, this, &RadarController::onRecvHeartbeatResponse);
    connect(m_tcpClient, &TcpClient::recvLoginResponse, this, &RadarController::onRecvLoginResponse);
    connect(m_tcpClient, &TcpClient::recvRTKData, this, &RadarController::onRecvRTKData);
    connect(m_tcpClient, &TcpClient::recvTrackDataV2, this, &RadarController::onRecvTrackDataV2);
    connect(m_tcpClient, &TcpClient::recvTrackDataV3, this, &RadarController::onRecvTrackDataV3);
    connect(m_tcpClient, &TcpClient::recvRegResponse, this, &RadarController::onRecvConfigParm);

    m_tcpClient->connectToServer(m_ip, m_port);

    m_heartbeatTimer = new QTimer(this);
    connect(m_heartbeatTimer, &QTimer::timeout, this, &RadarController::onHeartbeatTimerTimeout);
    QTimer::singleShot(1000, [=](){
        m_heartbeatTimer->start(1000);
    });
}

void RadarController::getConfigParm()
{
    GetRegRequestMessage message;
    message.registerIds.append(0x00000440);
    message.registerIds.append(0x00000401);

    m_tcpClient->sendMessage(MessageType::GetRegRequest, message.dumpBody());
}

void RadarController::setConfigParm(RadarConfigParm parm)
{
    uint32_t value = (((uint32_t)parm.freq & 0xFF) << 24)
                      | (((uint32_t)parm.speed & 0xFF) << 16)
                      | (((uint32_t)parm.cfar & 0xFF) << 8)
                      | ((uint32_t)parm.rcs & 0xFF);

    SetRegRequestMessage message;
    message.registers.append(QPair<uint32_t, uint32_t>(0x00000440, value));

    //qDebug()<<"set config parm:"<<parm.freq<<parm.speed<<parm.cfar<<parm.rcs;
    QString str;
    str.append("set config parm:");
    str.append(parm.freq);
    str.append(parm.speed);
    str.append(parm.cfar);
    str.append(parm.rcs);
    emit sendMessage(str);

    m_tcpClient->sendMessage(MessageType::SetRegRequest, message.dumpBody());
}

void RadarController::getRTK(bool status)
{
    RTKRequestMessage message;
    if (status) message.command = 0x01;
    else message.command = 0x00;

    m_tcpClient->sendMessage(MessageType::RTKRequest, message.dumpBody());
}

void RadarController::run(WorkMode mode)
{
    uint32_t rotationalSpeed = 0;
    uint32_t workingMode = 0;

    switch (mode) {
    case WorkMode::StandBy:
    {
        workingMode = 0;
        break;
    }
    case WorkMode::CirSweep1s:
    {
        rotationalSpeed = 0x04;
        workingMode = 1;
        break;
    }
    case WorkMode::CirSweep2s:
    {
        rotationalSpeed = 1;
        workingMode = 1;
        break;
    }
    case WorkMode::FanSweep:
    {

        break;
    }
    default:
        break;
    }

    uint32_t value = (((uint32_t)0 & 0xFF) << 24)
                      | (((uint32_t)0 & 0xFF) << 16)
                      | (((uint32_t)rotationalSpeed & 0xFF) << 8)
                      | ((uint32_t)workingMode & 0xFF);

    SetRegRequestMessage message;
    message.registers.append(QPair<uint32_t, uint32_t>(0x00000401, value));

    m_tcpClient->sendMessage(MessageType::SetRegRequest, message.dumpBody());
}

void RadarController::login()
{
    LoginRequestMessage message;
    message.role = 0x00000003;
    message.code = 0x00000000;

    m_tcpClient->sendMessage(MessageType::LoginRequest, message.dumpBody());
}

uint32_t RadarController::heartbeat()
{
    HeartbeatMessage message;
    message.timestamp = 0;

    uint32_t id = m_tcpClient->sendMessage(MessageType::HeartbeatRequest, message.dumpBody());
    m_heartbeatRequestSet.insert(id);

    return id;
}

void RadarController::onRecvLoginResponse()
{
    m_isLogin = true;
    m_timeoutCount = 0;
}

void RadarController::onRecvHeartbeatResponse(uint32_t id)
{   qDebug()<<"心跳包"<<endl;
    if (m_heartbeatRequestSet.contains(id))
        m_heartbeatRequestSet.remove(id);
}

void RadarController::onRecvConfigParm(GetRegResponseMessage regMessage)
{   qDebug()<<"配置信息"<<endl;
    foreach (auto reg, regMessage.registers)
    {
        switch (reg.first) {
        case 0x00000440:
        {
            m_config.freq = (reg.second >> 24) & 0xFF;
            m_config.speed = (reg.second >> 16) & 0xFF;
            m_config.cfar = (reg.second >> 8) & 0xFF;
            m_config.rcs = reg.second & 0xFF;
            break;
        }
        case 0x00000401:
        {
            uint8_t workingMode = reg.second & 0xFF;
            uint8_t rotationalSpeed = (reg.second >> 8) & 0xFF;

            if(workingMode == 0x00) //待机
            {
                m_mode = WorkMode::StandBy;
            }
            else if (workingMode == 0x01 && rotationalSpeed == 0x04) //周扫1s
            {
                m_mode = WorkMode::CirSweep1s;
            }
            else if (workingMode == 0x01 && rotationalSpeed == 0x01) //周扫2s
            {
                m_mode = WorkMode::CirSweep2s;
            }
            else if (workingMode == 0x02) //扇扫
            {
                m_mode = WorkMode::FanSweep;
            }
            break;
        }
        default:
            break;
        }
    }

    qDebug()<<"receive config parm:" << m_config.freq << m_config.speed << m_config.cfar << m_config.rcs << (int)m_mode;
}

void RadarController::onRecvRTKData(RTKDataMessage rtkMessage)
{
    qDebug()<<"RTKDATA"<<endl;
    // 单帧RTK数据误差偏大，可采取多帧取平均值的方法减小误差，建议取20帧（1帧/s）
    // 当不在需要RTK数据时，请使用getRTK(false)关闭，否则该回调函数将持续接收RTK数据

    qDebug()<<"receive RTK data, lat:"<< (double)rtkMessage.lat / 1e9
             << ",lon:" << (double)rtkMessage.lon / 1e9
             << ",azi:" << (double)rtkMessage.azimuth / 1e9;
}

//槽函数
void RadarController::onRecvTrackDataV2(TrackDataV2Message trackData)
{
    emit sendPoints(trackData);
    // G系列雷达登录后，将持续定时发送v2版本的数据包（本Demo在TCP建立后自动登录）
    // 雷达上传数据均为整数，请按照分辨率除某个值，如：(double)point.angle/1e4
    qDebug()<<"recvDataV2"<<endl;
//    foreach (auto point, trackData.points) {
//       // qDebug()<<"receive track data(v2),id:" << point.id << " xyz:" << point.x << point.y << point.z;
//        qDebug()<<"receive track data(v2),id:" << point.id
//                 << " azimuth:" << (double)point.angle/1e4
//                 << " distance:" << (double)point.distance/1e2
//                 << " speed:" << (double)point.speed/1e2
//                 << " snr:" << (double)point.snr/1e2;
//    }




}

void RadarController::onRecvTrackDataV3(TrackDataV3Message trackData)
{
    qDebug()<<"接受datav3"<< trackData.points.size();
    foreach (auto point, trackData.points) {
        qDebug()<<"receive track data(v3),id:" << point.id << " xyz:" << point.x << point.y << point.z;

        QString str;
        str.sprintf("receive track data(v3),id:%d xyz position:%d %d %d\n",point.id,point.x,point.y,point.z);
        sendMessage(str);
        //sendPoints(trackData);
    }

    // 协议中提供的是基于雷达的直角坐标系（转台零位为水平零度）的xyz坐标
    // 直角坐标转极坐标的方法：
    /**
     * 目标角度及其他信息的计算方式如下：
     * 		目标水平距离 horizontalRange = sqrt(x*x + y*y);
     * 		目标轴向距离 range = sqrt(x*x + y*y + z*z)
     * 		目标方位角 azimuth = atan2(x, y) / M_PI * 180.0;
     * 		目标俯仰角 pitch = asin(z / range) / M_PI * 180.0;
     * 		目标速度 speed = sqrt(vx*vx + vy*vy + vz*vz);
     */
}

void RadarController::onHeartbeatTimerTimeout()
{
    if (m_isLogin) //登录状态：通过心跳包确保连接状态正常
    {
        uint32_t id = heartbeat();
        QTimer::singleShot(1000, this, [=](){
            if (m_heartbeatRequestSet.contains(id))
            {
                m_timeoutCount++;
                m_heartbeatRequestSet.remove(id);
            }
            else
            {
                m_timeoutCount = 0;
            }

            // 连续两次心跳包响应超时，认为连接断开，重新建立连接
            if (m_timeoutCount > 1)
            {
                m_isLogin = false;
                m_tcpClient->disconnectFromServer();
            }
        });
    }
    else //未登录状态：重新建立连接并登录
    {
        if (m_tcpClient->state() == QAbstractSocket::SocketState::UnconnectedState)
        {
            m_tcpClient->connectToServer(m_ip, m_port);
        }
        else if (m_tcpClient->state() != QAbstractSocket::SocketState::ConnectedState)
        {
            m_tcpClient->disconnectFromServer();
        }
    }
}
